#include "ros/ros.h"
#include "std_msgs/String.h"
#include "jason_msgs/action.h"
#include "jason_msgs/event.h"

#include <boost/format.hpp>

ros::Subscriber sub;
ros::Publisher pub;

/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */
void actionCallback(const jason_msgs::action::ConstPtr& msg)
{
  ROS_INFO("demo_decomposer : a Jason action \"%s\" has been received from %s\n",  msg->action.c_str(), msg->agent.c_str() );
  if ( msg->action == "move_forward" ) {
      
      ROS_INFO("sleeping");
      sleep(5);
      ROS_INFO("Done");
      
      jason_msgs::event e;
      e.agent = msg->agent;
      e.event_type = "ActionFeedback";
      e.parameters.push_back( msg->action );
      e.parameters.push_back( boost::str( boost::format( "%d" ) % msg->action_id ) );
      e.parameters.push_back( "Ok" );
      
      pub.publish( e );
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "demo_decomposer");

  ros::NodeHandle n;

  sub = n.subscribe("/jason/action", 100, actionCallback);
  pub = n.advertise<jason_msgs::event>( "/jason/event", 10 );

  ros::spin();

  return 0;
}
